%%% find interest point in 3D point cloud
%   PX,PY,PZ - point of cloud 
%   ind - linear indice of the image
%   source - start searching point
function p = interestPointFinding(PX,PY,PZ,ind,thre)

    k = 10; % number of iteration
    p = zeros(1,k); % interest point set
%     profile on;
    Adjacency = createMesh(PX,PY,PZ,ind,thre);
%     profile viewer;
    s = initStartPoint(PX,PY,PZ);
    bs = s;
    distM = ones(size(PX(:)'))*inf;
    for k = 1:10
        dist = dijkstra(Adjacency,s);
        distM = [distM;dist];
        dist = min(distM);
        dist(dist == inf) = -1;
        [m,i] = max(dist);
        p(k) = i;
        s = i;
    end
    plot3(PX,PY,PZ,'.');
    hold on;
    plot3(PX(p(:)),PY(p(:)),PZ(p(:)),'r+')
    p = p(:);
    for i = 1:size(p,1)
        text(PX(p(i)),PY(p(i)),PZ(p(i)),int2str(i),'Color','r');
    end
    plot3(PX(bs),PY(bs),PZ(bs),'ro')
    axis equal;
    set(gca,'ZDir','reverse');
end

function s = initStartPoint(PX,PY,PZ)
    sx = mean(PX);
    sy = mean(PY);
    sz = mean(PZ);
    [m,s] = min(sqrt((PX - sx).^2 + (PY - sy).^2 + (PZ - sz).^2));
end

function Adjacency = createMesh(X,Y,D,ind,thre)
%%% Generate Adjacency matrix
%   when points are neighbors in distance map
%   and their 3D distance is less than a threshold

    % init as a sparse matrix(lower left corner)
    [x,y] = meshgrid(1:size(ind,1));
    x = x(:);
    y = y(:);
    
    % x,y entry in Adjacency matrix
    m = x < y;
    x = x(m);
    y = y(m);
    
    % get the adjacent pixels
    indx = ind(x);
    indy = ind(y);
    d = abs(indx - indy);
    x = x(d == 1|d == 144);
    y = y(d == 1|d == 144);
    Dist = getEuclideanDistance(X(x),X(y),...
        Y(x),Y(y),...
        D(x),D(y)); 
    x = x(Dist < thre);
    y = y(Dist < thre) ;
    
    % filter when distance > thre
    Dist = Dist(Dist <= thre);
    % check distance in 3D
    Adjacency = accumarray([x,y],Dist,[size(ind,1),size(ind,1)],@sum,0,true);
end

function Dist = getEuclideanDistance(X1,Y1,Z1,X2,Y2,Z2)
    Dist = sqrt((X1 - X2).^2 + (Y1 - Y2).^2 + (Z1 - Z2).^2);
end